/*
 * Copyright (c) 2011 Jean-Marc Perronne
 * Ensisa, 12 rue des Freres Lumieres, F-68093 Mulhouse cedex
 * All rights reserved.
 *
 * The copyright of this software is owned by Werner Randelshofer. 
 * You may not use, copy or modify this software, except in  
 * accordance with the license agreement you entered into with  
 * Jean-Marc Perronne. For details see accompanying license terms. 
 */

package fr.uha.ensisa.jmp.drone.handler;

import java.io.IOException;
import java.net.SocketException;
import java.net.SocketTimeoutException;
import java.net.UnknownHostException;
import java.nio.ByteBuffer;

import fr.uha.ensisa.jmp.drone.app.DroneConstants;
import fr.uha.ensisa.jmp.drone.at_commands.AtCommands;
import fr.uha.ensisa.jmp.drone.com.BadInterfaceException;
import fr.uha.ensisa.jmp.drone.com.NavdataConnection;
import fr.uha.ensisa.jmp.drone.navdata.Navdata;
import fr.uha.ensisa.jmp.drone.navdata.NavdataParser;


public class NavdataHandler extends ChildDroneHandler<Navdata>
{
	private NavdataConnection com;

	public NavdataHandler(DroneHandler drone) throws SocketException, UnknownHostException, BadInterfaceException
	{
		super(drone);
		this.com = new NavdataConnection();
	}

	protected void navdataWakeUp() throws IOException
	{
		//this.com.send("Hi".getBytes());
	    this.com.send(DroneConstants.WAKEUP_BYTES);
	}

	@Override
	public void run()
	{
		int navdataSequenceNumber = 0;
		NavdataParser navdataParser= new NavdataParser();
		try
		{
			this.navdataWakeUp();
			this.running();
			while(this.isActive() && this.drone.isActive())
			{
				try
				{
					ByteBuffer buffer = this.com.receive();
					navdataParser.preparse(buffer);
					
					if (navdataParser.isValidHeader())
					{		
						//System.out.println(navdataParser);
	
						if (navdataParser.isComWatchdogEnabled())
						{
							//System.out.println("com wdog enabled");
							navdataSequenceNumber = 0;
							if (!navdataParser.isInBootstrapMode() && this.drone.booted() )
							{
								//this.atHandler.sendImmediately(AtCommands.COM_WDG_CDE);
								this.drone.sendScheduled(AtCommands.COM_WDG_CDE);
							}
						}
						else
						{
							if(navdataParser.isSequenceNumberGreaterThan(navdataSequenceNumber))
							{
								if (navdataParser.isInNavDemoMode())
								{
									//System.out.println("drone in navdata demo mode");
	
									if (!navdataParser.parseOptions(buffer))
									{
										System.err.println("Checksum error");
									}
								}
							}
							else
							{
								System.err.println("Sequence number error, new ("+navdataParser.sequenceNumber()+") old("+navdataSequenceNumber+')');
							}
							navdataSequenceNumber = navdataParser.sequenceNumber();
						}
						this.data().add(navdataParser.navadata());
					}
					Thread.yield();
				}
				catch (SocketTimeoutException e)
				{
					//no data received, have to solicit drone
					e.printStackTrace();
					navdataSequenceNumber = 0;
					this.navdataWakeUp();
				}
			}
		}
		catch (IOException e)
		{
			System.err.println("nav wakeup failed");
			e.printStackTrace();
		}
		finally
		{
			this.terminate();
			this.notRrunning();
		}
	}

	public void start()
	{
		super.start();
		new Thread(this).start();
	}

	@Override
	public void close()
	{
		this.com.close();
	}

}
